# Paste your code.
import math
import domination
import cmath
from domination.libs import astar
astar = astar.astar
class CpData:
    def __init__(self, label, loc, owned):
        self.label = label
        self.loc = loc
        self.owned = owned
class ApData:
    def __init__(self, label, loc, hasAmmo, turn):
        self.label = label
        self.loc = loc
        self.hasAmmo = hasAmmo
        self.hasAmmoUpdated = turn
class SpawnData:
    def __init__(self, label, loc):
        self.label = label
        self.loc = loc
class FoeData:
    def __init__(self, grid, settings, label, loc, angle, turn):
        self.label = label
        self.settings = settings
        self.grid = grid
        self.oldLoc = None
        self.oldLocUpdated = turn
        self.loc = loc
        self.locUpdated = turn
        self.angle = angle
        self.angleUpdated = turn
        self.threat = 0
        self.threatUpdated = turn
        self.predictedTarget = None
        self.predictedTargetUpdated = turn
    def incThreat(self, turn):
        self.threat = min(self.threat + 1, 4)
        self.threatUpdated = turn
    def setLoc(self, loc, angle, turn):
        oldLoc = self.loc
        self.loc = loc
        self.locUpdated = turn
        self.angle = angle
        self.angleUpdated = turn
        if ( self.oldLocUpdated == turn - 1 ):
            self.oldLoc = oldLoc
        else:
            self.oldLoc = None
        self.oldLocUpdated = turn
    def predictTarget(self, turn):
        if ( self.predictedTargetUpdated == turn ):
            return
        if ( self.locUpdated == turn and self.oldLocUpdated == turn and self.oldLoc != None ):
            self.predictedTarget = None
            maxR = max(self.settings.max_range, self.settings.max_speed)
            theta = self.settings.max_turn
            objects = Agent.cps
            for l, a in Agent.aps.items():
                if ( a.hasAmmo ):
                    objects[l] = a
            for l, f in Agent.friends.items():
                objects[l] = f
            for l, o in objects.items():
                k = o.loc
                if line_intersects_grid(k, self.loc, self.grid, self.settings.tilesize):
                    continue
                objectPos = complex(k[0] - self.loc[0], k[1] - self.loc[1])
                (objR, objPhi) = cmath.polar(objectPos)    
                if ( objR < maxR and objPhi > self.angle - theta and objPhi < self.angle + theta ):
                    if (self.predictedTarget == None or objR < minR):
                        self.predictedTarget = l
                        minR = objR
            self.predictedTargetUpdated = turn
class Agent(object):
    NAME = "T-690"
    cps = { 'CP1' : CpData   (label='CP1', loc = (216, 56), owned = None),
            'CP2' : CpData   (label='CP2', loc = (248, 216), owned = None) }
    aps = { 'AP1' : ApData   (label='AP1', loc = (152, 136), hasAmmo = True, turn = 1),
            'AP2' : ApData   (label='AP2', loc = (312, 136), hasAmmo = True, turn = 1) }
    spawns={'BS1' : SpawnData(label='BS1', loc = (440,120)),
            'BS2' : SpawnData(label='BS2', loc = (440,136)),
            'BS3' : SpawnData(label='BS3', loc = (440,152)),
            'RS1' : SpawnData(label='RS1', loc = (24,120)),
            'RS2' : SpawnData(label='RS2', loc = (24,136)),
            'RS3' : SpawnData(label='RS3', loc = (24,152)) }
    foeNameCounter = 0
    foes = {}
    friends = {}
    helps = []
    width = 0
    widthConservative = 0
    @staticmethod
    def linkWithFoeLabel(foe):
        matches = []
        for l, oFoe in Agent.foes.items():
            if point_dist((foe[0], foe[1]), oFoe.loc) > oFoe.settings.max_speed:
                continue
            phi = math.atan2((foe[1]-oFoe.loc[1]),float((foe[0]-oFoe.loc[0])))
            eps = 0.1
            if inCone(foe[2], eps, phi) == 0:
                matches.append(l)
        return matches
    @staticmethod
    def updateFoes(observedFoes, settings, turn, grid):
        possibleFoes = []
        for observedFoe in observedFoes: 
            possibleFoes.append(Agent.linkWithFoeLabel(observedFoe))
        labelsDone = []
        newFoes = []
        size = len(possibleFoes) + 1
        while size != len(possibleFoes):
            size = len(possibleFoes)
            k = 0
            while k < len(possibleFoes):
                observedFoe = observedFoes[k]
                matchingFoes = possibleFoes[k]
                for label in labelsDone:
                    if label in matchingFoes:
                        matchingFoes.remove(label)
                if len(matchingFoes) == 0:
                    foeLabel = "FO" + str(Agent.foeNameCounter)
                    Agent.foeNameCounter = Agent.foeNameCounter + 1
                    newFoes.append(FoeData(label=foeLabel,loc=(observedFoe[0], observedFoe[1]),angle = observedFoe[2],turn = turn, settings = settings, grid = grid))
                    del possibleFoes[k]
                    del observedFoes[k]
                    continue
                if ( len(matchingFoes) == 1 ):
                    Agent.foes[matchingFoes[0]].setLoc((observedFoe[0], observedFoe[1]),observedFoe[2],turn)
                    labelsDone.append(matchingFoes[0])
                    del possibleFoes[k]
                    del observedFoes[k]
                else:
                    k = k+1
        for foe in newFoes:
            Agent.foes[foe.label] = foe
    @staticmethod
    def findCp(loc):
        for l, cp in Agent.cps.items():
            if loc[0] <= cp.loc[0] + 8 and loc[0] >= cp.loc[0] - 8 \
            and loc[1] <= cp.loc[1] + 8 and loc[1] >= cp.loc[1] - 8:
                return l
    @staticmethod
    def findAp(loc):
        for l, ap in Agent.aps.items():
            if loc[0] <= ap.loc[0] + 8 and loc[0] >= ap.loc[0] - 8 \
            and loc[1] <= ap.loc[1] + 8 and loc[1] >= ap.loc[1] - 8:
                return l
    @staticmethod
    def updateCps(cps):
        for cp in cps:
            cpLabel = Agent.findCp( (cp[0],cp[1]) )
            Agent.cps[cpLabel].owned = cp[2]
    @staticmethod
    def updateAps(seeDistance, agentLoc, aps, turn):
        for ap in Agent.aps:
            if Agent.aps[ap].hasAmmoUpdated != turn and point_dist(agentLoc, Agent.aps[ap].loc) < seeDistance:
                for apo in aps:
                    if Agent.aps[ap].loc[0] == apo[0] and Agent.aps[ap].loc[1] == apo[1]:
                        Agent.aps[ap].hasAmmo = True
                    else:
                        Agent.aps[ap].hasAmmo = False
                    Agent.aps[ap].hasAmmoUpdated = turn
        for ap in aps:
            apLabel = Agent.findAp( (ap[0],ap[1]) )
            Agent.aps[apLabel].hasAmmo = True
        pass
    def __init__(self, id, team, settings=None, field_rects=None, field_grid=None, nav_mesh=None, blob=None, matchinfo = None, **kwargs):
        self.id = id
        self.team = team
        self.mesh = nav_mesh
        self.grid = field_grid
        self.settings = settings
        self.goal = None
        self.callsign = '%s-%d'% (('BLU' if team == TEAM_BLUE else 'RED'), id)
        if self.id == 0:
            Agent.width = (2**0.5)*self.settings.tilesize
            Agent.widthConservative = self.settings.tilesize - 5
            self.label = "SK0"
            if self.team == TEAM_RED:
                self.currentBehaviour = "DefCP1"
            else:
                self.currentBehaviour = "DefCP2"
        elif self.id == 1:
            self.label = "SK1"
            self.currentBehaviour = "AmmoColl"
        else:
            self.label = "SK2"
            if self.team == TEAM_RED:
                self.currentBehaviour = "DefCP2"
            else:
                self.currentBehaviour = "DefCP1"
        self.currentTBehaviour = None
        self.currentSecondaryBehaviour = None
        self.currentSecondaryTarget = None
    def observe(self, observation):
        self.observation = observation
        self.selected = observation.selected
        self.loc = self.observation.loc
        self.angle = self.observation.angle
        if self.id == 0:
            Agent.friends = {}
            Agent.observedFoes = []
        Agent.friends["SK"+str(self.id)] = self
        for o in self.observation.foes:
            if o not in Agent.observedFoes:
                Agent.observedFoes.append(o)
        if self.id == 0:
            Agent.updateCps(self.observation.cps)
        Agent.updateAps(self.settings.max_see, self.loc, filter(lambda x: x[2] == "Ammo", self.observation.objects), self.observation.step)
    def action(self):
        if self.id == 0:
            Agent.updateFoes(observedFoes = Agent.observedFoes, turn = self.observation.step, settings = self.settings, grid = self.grid)
            for l, f in Agent.foes.items():
                if ( f.locUpdated != self.observation.step ):
                    del Agent.foes[l]
        if self.observation.respawn_in > 0:
            self.currentTBehaviour = None
            return (0,0,False)
        if self.observation.ammo > 0:
            for l, foe in Agent.foes.items():
                if self.inSingleRange(foe.loc):
                    return self.EngageFoe(l)
        (ap, d) = self.findNearestAmmo()
        if ap is not None:
            (result, move) = self.inSingleMove(Agent.aps[ap].loc)
            if result:
                return move
        if self.currentBehaviour == "AmmoColl" and self.observation.ammo > 0:
            if len(Agent.helps) > 0:
                (label, friend) = Agent.helps.pop(0)
                label = self.getRelativeMap(label) 
                self.currentBehaviour = "Def"+label
                self.currentTBehaviour = None
                friend.currentBehaviour = "AmmoColl"
        return getattr(self, self.currentBehaviour)()
    def AmmoColl(self):
        if self.observation.ammo > 0:
            (foe, steps) = self.findNearestFoe()
            if foe is not None and steps < 2:
                return self.EngageFoe(foe)
        (a,d) = self.findNearestAmmo()
        if a is not None:
            self.currentTBehaviour = a
        ap2 = self.getRelativeMap("AP2")
        ap1 = self.getRelativeMap("AP1")
        if self.currentTBehaviour == ap2:
            ap1, ap2 = ap2, ap1
        if self.loc == Agent.aps[ap1].loc:
            self.currentTBehaviour = ap2
            return self.DefendAmmopoint(ap2)
        else:
            return self.DefendAmmopoint(ap1)
    def DefCP1(self):
        cp1 = self.getRelativeMap("CP1")
        return self.DefendCheckpoint(cp1)
    def DefCP2(self):
        cp2 = self.getRelativeMap("CP2")
        return self.DefendCheckpoint(cp2)
    def TurnTo(self, target):
        dx = target[0] - self.loc[0]
        dy = target[1] - self.loc[1]
        turn = angle_fix(math.atan2(dy, dx) - self.angle)
        return (turn, 0, False)
    def MoveTo(self, target):
        path, length, direct = myFindPath(self.loc, target, self.mesh, self.grid, self.settings.tilesize)
        if path:
            dx = path[0][0] - self.loc[0]
            dy = path[0][1] - self.loc[1]
            desiredAngle = math.atan2(dy, dx)
            turn = angle_fix(desiredAngle - self.angle)
            distance = (dx**2 + dy**2)**0.5
            if turn > self.settings.max_turn or turn < -self.settings.max_turn:
                absMissing = math.fabs(turn)-self.settings.max_turn
                if absMissing <= self.settings.max_turn:
                    if not direct:
                        coneWidth = self.settings.max_turn
                        sStepAngle = self.angle + getAngle(path[0], path[1])
                        result = inCone(desiredAngle, coneWidth, sStepAngle)
                    else:
                        result = desiredAngle 
                    if result * desiredAngle <= 0 and distance <= self.settings.max_speed:
                        speed = 0
                    else:
                        xDiffRot= dx*math.cos(-self.angle) - dy*math.sin(-self.angle)
                        speed = math.fabs(xDiffRot)*(2**0.5)
                else:
                    backTurn = angle_fix(math.atan2(-dy, -dx) - self.angle)
                    if direct and distance < self.settings.max_speed and \
                       ( backTurn > self.settings.max_turn or backTurn < -self.settings.max_turn ):
                        turn = backTurn
                        if self.currentSecondaryBehaviour != "EngageFoe":
                            speed = -distance
                            turn = angle_fix(turn-math.copysign(0.18, turn))
                        else:
                            speed = -self.settings.max_speed
                    else:
                        xDiffRot= dx*math.cos(-self.angle) - dy*math.sin(-self.angle)
                        reducer = distance//self.settings.max_speed + 2
                        speed = max((-math.fabs(xDiffRot)*(2**0.5)), -self.settings.max_speed)
                        speed = speed / reducer
            else:
                speed = distance
        else:
            dx = target[0] - self.loc[0]
            dy = target[1] - self.loc[1]
            turn = angle_fix(math.atan2(dy, dx) - self.angle)
            speed = (dx**2 + dy**2)**0.5
        return (turn,speed, False)
    def EngageFoe(self,l):
        shootAction = False
        distanceAction = 0
        foe = Agent.foes[l]
        distanceToTarget = point_dist(self.loc, foe.loc)
        if ( distanceToTarget > self.settings.max_range ) or ( line_intersects_grid(self.loc,foe.loc,self.grid,self.settings.tilesize)):
            return self.MoveTo(foe.loc)
        angleWithTarget = angle_fix(getAngle(self.loc, foe.loc) - self.angle)
        epsShoot = math.asin(Agent.widthConservative/(2*distanceToTarget))
        angleAction = math.copysign(min(math.fabs(angleWithTarget),self.settings.max_turn),angleWithTarget)
        if inCone(angleWithTarget, epsShoot, angleAction) != 0:
            distanceAction = 0
        else :
            shootAction = True
            for l, friend in Agent.friends.items():
                if friend != self:
                    distanceToFriend = point_dist(self.loc,friend.loc)
                    if distanceToFriend < distanceToTarget:
                        angleWithFriend = angle_fix(getAngle(self.loc,friend.loc) - self.angle)
                        eps = math.asin(Agent.width/(2*distanceToFriend))
                        if inCone(angleAction, eps, angleWithFriend) == 0:
                            shootAction = False
                            break
        return (angleAction, distanceAction, shootAction)
    def EvadeFoe(self):
        pass
    def CollectAmmo(self, label):
        return self.MoveTo(Agent.aps[label].loc)
    def ActivateCheckpoint(self, label):
        return self.MoveTo(Agent.cps[label].loc)
    def DefendCheckpointSecondary(self, label):
        cp = Agent.cps[label]
        defendThreshold = 4
        (nearestFoe, dist) = self.findNearestFoeToLoc(cp.loc, defendThreshold)
        if ( nearestFoe is not None ) and ( not line_intersects_grid(cp.loc,Agent.foes[nearestFoe].loc,self.grid,self.settings.tilesize)):
            if self.observation.ammo == 0:
                if (label, self) not in Agent.helps:
                    Agent.helps.append((label, self))
            else:
                return ("EngageFoe", nearestFoe)
        else:
            if ( label,self ) in Agent.helps:
                Agent.helps.remove( (label,self) )
        if not cp.owned == self.team:
            return ("ActivateCheckpoint",label)
        return (None, None)
    def DefendCheckpoint(self, label):
        (self.currentSecondaryBehaviour, self.currentSecondaryTarget) = \
            self.DefendCheckpointSecondary(label)
        if self.currentSecondaryBehaviour is not None:
            return getattr(self, self.currentSecondaryBehaviour)(self.currentSecondaryTarget)
        cp = Agent.cps[label]
        if self.loc != cp.loc:
            return self.MoveTo(cp.loc)
        (nearestFoe, dist) = self.findNearestFoe()
        if nearestFoe is not None:
            return self.TurnTo(Agent.foes[nearestFoe].loc)
        if self.team == TEAM_RED:
            return self.TurnTo(Agent.spawns['BS2'].loc)
        else:
            return self.TurnTo(Agent.spawns['RS2'].loc)
    def DefendAmmopointSecondary(self, label):
        ap = Agent.aps[label]
        if self.observation.ammo != 0:
            defendThreshold = 4
            (nearestFoe, dist) = self.findNearestFoeToLoc(ap.loc, defendThreshold)
            if nearestFoe is not None:
                return ("EngageFoe", nearestFoe)
        return (None, None)
    def DefendAmmopoint(self, label):
        (self.currentSecondaryBehaviour, self.currentSecondaryTarget) = \
            self.DefendAmmopointSecondary(label)
        if self.currentSecondaryBehaviour is not None:
            return getattr(self, self.currentSecondaryBehaviour)(self.currentSecondaryTarget)
        ap = Agent.aps[label]
        if self.loc != ap.loc:
            return self.MoveTo(ap.loc)
        (nearestFoe, dist) = self.findNearestFoe()
        if nearestFoe is not None:
            return self.TurnTo(Agent.foes[nearestFoe].loc)
        if self.team == TEAM_RED:
            return self.TurnTo(Agent.spawns['BS2'].loc)
        else:
            return self.TurnTo(Agent.spawns['RS2'].loc)
    def inSingleMove(self, t):
        dx = t[0] - self.loc[0]
        dy = t[1] - self.loc[1]
        distance = (dx**2 + dy**2)**0.5
        if distance > self.settings.max_speed:
            return (False,(0, 0, False))
        turn = angle_fix(math.atan2(dy, dx) - self.angle)
        if turn < self.settings.max_turn and turn > -self.settings.max_turn:
            return (True,(turn, distance, False))
        revTurn = angle_fix(math.atan2(dy,dx) - self.angle)
        if revTurn < self.settings.max_turn and revTurn > -self.settings.max_turn:
            return (True,(revTurn, -distance, False))
        return (False,(0,0, False))
    def inSingleRange(self, loc):
        if point_dist(self.loc, loc) > self.settings.max_range:
            return False
        dx = loc[0] - self.loc[0]
        dy = loc[1] - self.loc[1]
        turn = angle_fix(math.atan2(dy, dx) - self.angle)
        if turn < self.settings.max_turn and turn > -self.settings.max_turn and \
           not line_intersects_grid(self.loc, loc, self.grid, self.settings.tilesize):
                return True
        return False
    def getRelativeMap(self, label):
        if self.team == TEAM_BLUE:
            if label == "CP1":
                label = "CP2"
            elif label == "CP2":
                label = "CP1"
            elif label == "AP1":
                label = "AP2"
            elif label == "AP2":
                label = "AP1"
        return label
    def getOtherAp(self, label):
        if label == "AP1":
            return "AP2"
        return "AP1"
    def findNearestAp(self, minSteps = 999999):
        resAP = None
        steps = 0
        for l, ap in Agent.aps.items():
            steps = self.getApproxSteps(ap.loc)
            if steps < minSteps:
                minSteps = steps
                resAP = l
        return (resAP, steps)
    def findNearestAmmo(self, minSteps = 999999):
        resAP = None
        steps = 0
        for l, ap in Agent.aps.items():
            if ( ap.hasAmmo and ap.hasAmmoUpdated == self.observation.step ):
                steps = self.getApproxSteps(ap.loc)
                if steps < minSteps:
                    minSteps = steps
                    resAP = l
        return (resAP, steps)
    def findNearestFoe(self, minSteps = 999999):
        resFoe = None
        steps = 0
        for l, foe in Agent.foes.items():
            steps = self.getApproxSteps(foe.loc)
            if steps < minSteps:
                minSteps = steps
                resFoe = l
        return (resFoe, steps)
    def findNearestFoeToLoc(self, loc, minSteps = 999999):
        resFoe = None
        steps = 0
        for l, foe in Agent.foes.items():
            steps = self.getApproxSteps(agent = foe, target = loc)
            if steps < minSteps:
                minSteps = steps
                resFoe = l
        return (resFoe, steps)
    def getApproxSteps(self, target, agent = None):
        if agent == None:
            agent = self
        (nodes, length, straightLine) = myFindPath(agent.loc, target, self.mesh, self.grid, self.settings.tilesize)
        dx = nodes[0][0] - self.loc[0]
        dy = nodes[0][1] - self.loc[1]
        angle = math.fabs(math.atan2(dy, dx) - agent.angle)
        return angle / self.settings.max_turn + length / self.settings.max_speed + len(nodes) - 1
    def finalize(self, interrupted=False):
        """ This function is called after the game ends, 
            either due to time/score limits, or due to an
            interrupt (CTRL+C) by the user. Use it to
            store any learned variables and write logs/reports.
        """
        pass
    def debug(self, surface):
        """ Allows the agents to draw on the game UI,
            Refer to the pygame reference to see how you can
            draw on a pygame.surface. The given surface is
            not cleared automatically. Additionally, this
            function will only be called when the renderer is
            active, and it will only be called for the active team.
        """
        import pygame
        if self.id == 0:
            surface.fill((0,0,0,0))
        if self.selected:
            if self.goal is not None:
                pygame.draw.line(surface,(0,0,0),self.loc, self.goal)
def getAngle(start, end):
    dx = end[0] - start[0]
    dy = end[1] - start[1]
    return math.atan2(dy, dx)
def inCone(coneDir, conWidth, angle):
    highBound = coneDir + conWidth
    lowBound = coneDir - conWidth
    if lowBound < -math.pi:
        angle = math.fmod(angle-2*math.pi, 2*math.pi) 
    elif highBound > math.pi:
        angle = math.fmod(angle+2*math.pi, 2*math.pi) 
    if angle < lowBound:
        return -1
    elif angle > highBound:
        return 1
    else:
        return 0
def myFindPath(start, end, mesh, grid, tilesize=16):
    """ Uses astar to find a path from start to end,
        using the given mesh and tile grid.
        >>> grid = [[0,0,0,0,0],[0,0,0,0,0],[0,0,1,0,0],[0,0,0,0,0],[0,0,0,0,0]]
        >>> mesh = make_nav_mesh([(2,2,1,1)],(0,0,4,4),1)
        >>> find_path((0,0),(4,4),mesh,grid,1)
        [(4, 1), (4, 4)]
    """
    if not line_intersects_grid(start, end, grid, tilesize):
        return ([end], point_dist(start, end), True)
    direct = False
    mesh = copy.deepcopy(mesh)
    mesh[start] = dict([(n, point_dist(start,n)) for n in mesh if not line_intersects_grid(start,n,grid,tilesize)])
    if end not in mesh:
        endconns = [(n, point_dist(end,n)) for n in mesh if not line_intersects_grid(end,n,grid,tilesize)]
        for n, dst in endconns:
            mesh[n][end] = dst
    neighbours = lambda n: mesh[n].keys()
    cost       = lambda n1, n2: mesh[n1][n2]
    goal       = lambda n: n == end
    heuristic  = lambda n: ((n[0]-end[0]) ** 2 + (n[1]-end[1]) ** 2) ** 0.5
    nodes, length = astar(start, neighbours, goal, 0, cost, heuristic)
    if len(nodes) == 1:
        direct = True
    return (nodes, length, direct)
